Singularity Avoidance as Manipulability Maximization Using Continuous Time Gaussian Processes

A significant challenge in motion planning is to avoid being in or near singular configurations, that is, configurations in joint space that result in the loss of the ability to move in certain directions in task space. A robotic system’s manipulability is reduced even in regions that are in close proximity to (i.e., neighbouring) a singularity. In this work we examine singularity avoidance in a motion planning context, that is, finding a trajectory which minimizes proximity to singular regions, subject to constraints. Representing the trajectory as a sample taken from a continuous time Gaussian process, we define a likelihood associated with singularity avoidance. We leverage recent work on motion planning using exactly sparse Gaussian processes and a factor graph representation to maximize the singularity avoidance likelihood using a maximum a posteriori (MAP) estimator. Viewing the MAP problem as inference on a factor graph, we use gradient information from interpolated states to maximize the trajectory’s overall manipulability. Both qualitative and quantitative analysis of experimental data show increases in manipulability which results in smooth trajectories with visibly more dexterous configurations.